#pragma once

#include <cmath>
#include <cassert>
#include <boost/operators.hpp>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795 
#endif

typedef double Scalar;
const Scalar EPSILON = 1e-5;
const Scalar EPSILON2 = (EPSILON * EPSILON);
const Scalar EPSILON4 = (EPSILON2 * EPSILON2);


class Vector2
{
public:
	Vector2() : x_(), y_() {}
	Vector2(Scalar x, Scalar y) : x_(x), y_(y) {}

	Vector2(const Vector2& rhs) : x_(rhs.x_), y_(rhs.y_) {}

	template<typename VectorExpr2>
	Vector2(const VectorExpr2& rhs)
	{
		(*this) = rhs;
	}

	Vector2& operator=(const Vector2& rhs)
	{
		x_ = rhs.x_;
		y_ = rhs.y_;
		return *this;
	}

	template<typename VectorExpr2>
	Vector2& operator=(const VectorExpr2& rhs)
	{
		rhs.assignTo(*this);
		return *this;
	}

	Scalar x() const {	return x_; }
	Scalar y() const {	return y_; }
	void setX(Scalar x) { x_ = x; }
	void setY(Scalar y) { y_ = y; }

	Vector2& operator+=(const Vector2& rhs)
	{
		x_ += rhs.x_;
		y_ += rhs.y_;
		return *this;
	}

	Vector2& operator-=(const Vector2& rhs)
	{
		x_ -= rhs.x_;
		y_ -= rhs.y_;
		return *this;
	}

	Vector2& operator*=(const Scalar rhs)
	{
		x_ *= rhs;
		y_ *= rhs;
		return *this;
	}

	Vector2& operator/=(const Scalar rhs)
	{
		x_ /= rhs;
		y_ /= rhs;
		return *this;
	}

	const Vector2& operator+() const
	{
		return *this;
	}

	bool normalize()
	{
		Scalar l = length();
		if (isZero(l))
		{
			return false;
		}
		else
		{
			(*this) /= l;
			return true;
		}
	}

	bool isZero() const	{	return isZero(norm());	}
	static bool isZero(Scalar norm) {	return (norm  <= EPSILON2);	}

	Scalar length() const
	{
		return std::sqrt(squaredLength());
	}
	Scalar squaredLength() const
	{
		return norm();
	}
	Scalar norm() const
	{
		return (x_ * x_) + (y_ * y_);
	}

private:
	Scalar x_;
	Scalar y_;
};


///////////////////
// operation (meta expression template)
template<class DerivedT>
class VectorExprBase2
{
public:
	operator Vector2() const
	{
		Vector2 result;
		static_cast<const DerivedT&>(*this).assginTo(result);
	}
};

class VectorNegation2 : public VectorExprBase2<VectorNegation2>
{
public:
	VectorNegation2(const Vector2& arg)
		: arg_(&arg)
	{}

	void assignTo(Vector2& result) const
	{
		result.setX(-arg_->x());
		result.setY(-arg_->y());
	}

private:
	const Vector2* arg_;
};

inline const VectorNegation2 operator-(const Vector2& arg)
{
	return VectorNegation2(arg);
}


class VectorAddition2 : public VectorExprBase2<VectorAddition2>
{
public:
	VectorAddition2(const Vector2& lhs, const Vector2& rhs)
		: lhs_(&lhs), rhs_(&rhs)
	{}

	void assignTo(Vector2& result) const
	{
		result = *lhs_;
		result += *rhs_;
	}

private:
	const Vector2* lhs_;
	const Vector2* rhs_;
};

inline const VectorAddition2 operator+(const Vector2& lhs, const Vector2& rhs)
{
	return VectorAddition2(lhs, rhs);
}

class VectorSubtraction2 : public VectorExprBase2<VectorSubtraction2>
{
public:
	VectorSubtraction2(const Vector2& lhs, const Vector2& rhs)
		: lhs_(&lhs), rhs_(&rhs)
	{}

	void assignTo(Vector2& result) const
	{
		result = *lhs_;
		result -= *rhs_;
	}

private:
	const Vector2* lhs_;
	const Vector2* rhs_;
};

inline const VectorSubtraction2 operator-(const Vector2& lhs, const Vector2& rhs)
{
	return VectorSubtraction2(lhs, rhs);
}

inline const Vector2 operator*(const Vector2& lhs, const Scalar rhs)
{
	Vector2 result(lhs);
	result *= rhs;
	return result;
}

inline const Vector2 operator*(const Scalar lhs, const Vector2& rhs)
{
	Vector2 result(rhs);
	result *= lhs;
	return result;
}
inline Scalar dot(const Vector2& lhs, const Vector2& rhs)
{
	return (lhs.x() * rhs.x() + lhs.y() * rhs.y());
}

inline Scalar det(const Vector2& lhs, const Vector2& rhs)
{
	return (lhs.x() * rhs.y() - lhs.y() * rhs.x());
}

///////////////////
class Matrix2x2
{
public:
	Matrix2x2(Scalar a00, Scalar a01, Scalar a10, Scalar a11)
	{
		rows_[0].setX(a00);
		rows_[0].setY(a01);
		rows_[1].setX(a10);
		rows_[1].setY(a11);
	}

	const Vector2 operator*(const Vector2& rhs) const
	{
		return Vector2(dot(rows_[0], rhs), dot(rows_[1], rhs));
	}

private:
	static const std::size_t DIMENSION = 2;
	Vector2	rows_[DIMENSION];
};
